// -*-C++-*-
/*!
 * @file  ForceSensorSVC_impl.cpp
 * @brief Service implementation code of ForceSensor.idl
 *
 */

#include <iostream>
#include <unistd.h>
#include <stdlib.h>
#include <sys/io.h>
#include "ForceSensorSVC_impl.h"

#define BASEADDR 0x314
#define G 9.8

#define OK 0
#define ERROR -1

extern int fs[6]; //フルスケール
extern int filter; //フィルタ
extern double tool[6]; //座標変換

/*
 * Example implementational code for IDL interface FS
 */
FSSVC_impl::FSSVC_impl()
{
    // Please add extra constructor code here.
}


FSSVC_impl::~FSSVC_impl()
{
    // Please add extra destructor code here.
}


/*
 * Methods corresponding to IDL attributes and operations
 */

void Convert_DIGItoNEWTON(double Value[6])
{                     //0.000061035 = 1/16384
    Value[0] = Value[0] * 0.000061035 * fs[0]/10  * G; //[N]
    Value[1] = Value[1] * 0.000061035 * fs[1]/10  * G; //[N]
    Value[2] = Value[2] * 0.000061035 * fs[2]/10  * G; //[N]
    Value[3] = Value[3] * 0.000061035 * fs[3]/100 * G; //[Nm]
    Value[4] = Value[4] * 0.000061035 * fs[4]/100 * G; //[Nm]
    Value[5] = Value[5] * 0.000061035 * fs[5]/100 * G; //[Nm]
}


CORBA::Short FSSVC_impl::Get_Wrench(FS::DoubleArray Force)
{
    double Value[6];

    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) {
        outw(0x90+filter+i,BASEADDR); //フィルタを見る
        Value[i] = inw(BASEADDR+2);

        if(Value[i]>32767)
            Value[i] = Value[i] - 65536;
    }

    Convert_DIGItoNEWTON(Value);
    for(int i=0; i<6; i++)
        Force[i] = Value[i];

    std::cout << "Get_Wrench OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_DigitalWrench(FS::DoubleArray Force)
{
    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) {
        outw(0xa0+i, BASEADDR);
        Force[i] = inw(BASEADDR+2);

        if(Force[i]>32767)
            Force[i] -= 65536;
    }

    std::cout << "Get_DigitalWrench OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Reset_Peak()
{
    int flag=0;

    ioperm(BASEADDR,4,1);

    outw(0x7f, BASEADDR);
    outw(0x90+filter, BASEADDR+2); //フィルタを見る
    outw(0xe7, BASEADDR);
    outw(0x0b00, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    std::cout << "Reset_Peak OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_MaxPeak(FS::DoubleArray Force)
{
    int flag=0;
    double Value[6];

    ioperm(BASEADDR,4,1);

    outw(0x7f, BASEADDR);
    outw(0x90+filter, BASEADDR+2); //フィルタを見る
    outw(0xe7, BASEADDR);
    outw(0x0c00, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    for(int i=0; i<6; i++) {
        outw(0xd8+i, BASEADDR);
        Value[i] = inw(BASEADDR+2);

        if(Value[i]>32767)
            Value[i] -= 65536;
    }

    Convert_DIGItoNEWTON(Value);
    for(int i=0; i<6; i++)
        Force[i] = Value[i];

    std::cout << "Get_MaxPeak OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_MinPeak(FS::DoubleArray Force)
{
    int flag=0;
    double Value[6];

    ioperm(BASEADDR,4,1);

    outw(0x7f, BASEADDR);
    outw(0x90+filter, BASEADDR+2); //フィルタを見る
    outw(0xe7, BASEADDR);
    outw(0x0c00, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    for(int i=0; i<6; i++) {
        outw(0xd0+i, BASEADDR);
        Value[i] = inw(BASEADDR+2);

        if(Value[i]>32767)
            Value[i] -= 65536;
    }

    Convert_DIGItoNEWTON(Value);
    for(int i=0; i<6; i++)
        Force[i] = Value[i];

    std::cout << "Get_MinPeak OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Set_Offset()
{
    int Force[6], Offset[6];
    int flag = 0;

    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) {
        outw(0xa0+filter+i, BASEADDR); //filterみてる
        Force[i] = inw(BASEADDR+2);
    }

    for(int i=0; i<6; i++) {
        outw(0x88+i, BASEADDR);
        Offset[i] = inw(BASEADDR+2);
    }

    for(int i=0; i<6; i++) {
        Offset[i] += Force[i];
        outw(0x88+i, BASEADDR);
        outw(Offset[i], BASEADDR+2);
    }

    outw(0xe7, BASEADDR);
    outw(0x0700, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    std::cout << "Set_Offset OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_Offset(FS::DoubleArray Offset)
{
    double Value[6];

    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) {
        outw(0x88+i, BASEADDR);
        Value[i] = (double)inw(BASEADDR+2);

        if(Value[i]>32767)
            Value[i] -= 65536;
    }

    Convert_DIGItoNEWTON(Value);
    for(int i=0; i<6; i++)
        Offset[i] = Value[i];

    std::cout << "Get_Offset OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Set_Fullscale(FS::ShortArray Force)
{
    int flag = 0;

    ioperm(BASEADDR,4,1);

    if(Force[0] < 2 || Force[1] < 2 || Force[2] < 3
       ||Force[3] < 1 || Force[4] < 1 || Force[5] < 1) {
        std::cout << "Set_Fullscale ERROR" << std::endl;
        return ERROR;
    }

    if(Force[0] > 1040 || Force[0] < 200 //実際は270。デフォルトに合わせてある
       ||Force[1] > 1020 || Force[1] < 200 //実際は260
       ||Force[2] > 2030 || Force[2] < 400 //実際は510
       ||Force[3] >   35 || Force[3] <   9
       ||Force[4] >   32 || Force[4] <   9
       ||Force[5] >   37 || Force[5] <  10) {
        std::cout << "Set_Fullscaler Over" << std::endl;
    }

    fs[0] = (int)(Force[0] * 10 / G / 2); //フルスケールを本来の定義まで戻す
    fs[1] = (int)(Force[1] * 10 / G / 2); //[N] ->[kgf*10]
    fs[2] = (int)(Force[2] * 10 / G / 2); //[Nm]->[kgfcm]
    fs[3] = (int)(Force[3] *100 / G / 2);
    fs[4] = (int)(Force[4] *100 / G / 2);
    fs[5] = (int)(Force[5] *100 / G / 2);

    for(int i=0; i<6; i++) {
        outw(0x80+i, BASEADDR);
        outw(fs[i], BASEADDR+2);
    }

    outw(0xe7, BASEADDR);
    outw(0x0a00, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    std::cout << "Set_Fullscale OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_Fullscale(FS::DoubleArray Force)
{
    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) {
        outw(0x80+i, BASEADDR);
        Force[i] = inw(BASEADDR+2);
    }

    Force[0] = Force[0] / 10 * G * 2; //Forceを[N][Nm]まで戻す
    Force[1] = Force[1] / 10 * G * 2; //[kgf*10]->[N]
    Force[2] = Force[2] / 10 * G * 2; //[kgfcm] ->[Nm]
    Force[3] = Force[3] /100 * G * 2;
    Force[4] = Force[4] /100 * G * 2;
    Force[5] = Force[5] /100 * G * 2;

    std::cout << "Get_Fullscale OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Set_Filter(CORBA::Short& num)
{
    if(num > 6 || num < 0) {
        std::cout << "Set_Filter ERROR" << std::endl;
        return ERROR;
    }

    filter = num * 8;

    std::cout << "Set_Filter OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_Filter(CORBA::Short& num)
{
    num = filter / 8;

    std::cout << "Get_Filter OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Set_ToolCoordinate(FS::DoubleArray Tool)
{
    int flag=0;

    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) //tool初期化
        tool[i] = 0;

    for(int i=0; i<3; i++) { //まずエラー判断
        if(fabs(Tool[i]) > 3276) {
            std::cout << "Set_ToolCoordinate ERROR" << std::endl;
            return -1;
        }
    }
    for(int i=3; i<6; i++) {
        if(fabs(Tool[i]) > 180) {
            std::cout << "Set_ToolCoordinate ERROR" << std::endl;
            return -1;
        }
    }

    for(int i=0; i<6; i++) //Toolをtoolに代入＆変換
        tool[i] = Tool[i];

    outw(0x0200, BASEADDR); //その後座標変換
    outw(1, BASEADDR+2);
    outw(0x0201, BASEADDR);
    outw((int)(tool[0]*10), BASEADDR+2);

    outw(0x0202, BASEADDR);
    outw(2, BASEADDR+2);
    outw(0x0203, BASEADDR);
    outw((int)(tool[1]*10), BASEADDR+2);

    outw(0x0204, BASEADDR);
    outw(3, BASEADDR+2);
    outw(0x0205, BASEADDR);
    outw((int)(tool[2]*10), BASEADDR+2);

    outw(0x0206, BASEADDR);
    outw(6, BASEADDR+2);
    outw(0x0207, BASEADDR);
    outw((int)(-tool[3]*32768/180), BASEADDR+2);

    outw(0x0208, BASEADDR);
    outw(5, BASEADDR+2);
    outw(0x0209, BASEADDR);
    outw((int)(-tool[4]*32768/180), BASEADDR+2);

    outw(0x020a, BASEADDR);
    outw(4, BASEADDR+2);
    outw(0x020b, BASEADDR);
    outw((int)(-tool[5]*32768/180), BASEADDR+2);

    outw(0x020c, BASEADDR);
    outw(0, BASEADDR+2);
    outw(0xe7, BASEADDR);
    outw(0x0500, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    std::cout << "Set_ToolCoordinate OK" << std::endl;
    return OK;
}

CORBA::Short FSSVC_impl::Get_ToolCoordinate(FS::DoubleArray Tool)
{
    for(int i=0; i<6; i++)
        Tool[i] = tool[i];

    std::cout << "Get_ToolCoordinate OK" << std::endl;
    return OK;
}



// End of example implementational code



